/*
 * Copyright (c) 2024 iSoftStone Education Co., Ltd.
 * Licensed under the Apache License, Version 2.0 (the "License");
 */

#include <stdio.h>
#include <stdint.h>
#include "los_task.h"
#include "ohos_init.h"
#include "iot_errno.h"
#include "iot_pwm.h"
#include<stdbool.h>

// 电机运动方向
typedef enum {
    MOTOR_DIR_UP = 0,
    MOTOR_DIR_DOWN = 1
} MotorDirection;

// 电机状态
typedef enum {
    MOTOR_STATE_IDLE = 0,
    MOTOR_STATE_RUNNING = 1,
    MOTOR_STATE_STOPPED = 2
} MotorState;

// 电机控制命令
typedef enum {
    MOTOR_CMD_STOP = 0,
    MOTOR_CMD_UP = 1,
    MOTOR_CMD_DOWN = 2
} MotorCommand;

// 步进电机控制引脚定义 - 根据E53接口定义修改
#define MOTOR_PIN_A EPWMDEV_PWM0_M1  // GPIO_PB4 - E53接口引脚20
#define MOTOR_PIN_B EPWMDEV_PWM1_M1  // GPIO_PB5 - E53接口引脚17
#define MOTOR_PIN_C EPWMDEV_PWM7_M1  // GPIO_PD0 - E53接口引脚8
#define MOTOR_PIN_D EPWMDEV_PWM2_M1  // GPIO_PB6 - E53接口引脚19

// 步进电机参数
#define STEPS_PER_REVOLUTION 8192    // 步进电机一圈的步数（28BYJ-48规格）
#define PULLEY_DIAMETER 2.0f         // 滑轮直径(cm)
#define PI 3.14159f
#define DISTANCE_PER_REVOLUTION (PI * PULLEY_DIAMETER)  // 每转一圈的距离(cm)

// PWM参数定义
#define PWM_FREQ 1000               // PWM频率100Hz
#define PWM_DUTY_ON 99            // 通电时占空比80%
#define PWM_DUTY_OFF 1             // 断电时占空比0%
#define STEP_DELAY_BASE 1        // 基础步进延时(ms)

// 电机控制相关变量
static MotorState g_motorState = MOTOR_STATE_IDLE;
static float g_currentHeight = 0.0f;
static float g_targetHeight = 0.0f;
static uint32_t g_currentStep = 0;
static MotorDirection g_currentDirection7 = MOTOR_DIR_DOWN;
static uint8_t g_currentSpeed = 5;

// 步进电机相序表 - 4拍单相激励方式
static const uint8_t STEP_SEQUENCE[4][4] = {
    {0, 0, 0, 1},  // A相通电
    {0, 0, 1, 0},  // B相通电
    {0, 1, 0, 0},  // C相通电
    {1, 0, 0, 0}   // D相通电
};

// 添加启动标志
static bool g_motorEnabled = false;

// 函数声明
static int32_t InitMotorPins(void);
static void SetMotorPhase(uint8_t phase);
static void StopMotor(void);
static void MotorControlTask7(void);
int32_t StepperMotorInit7(void);
int32_t StepperMotorControl7(MotorCommand command, float targetHeight, uint8_t speed);
MotorState StepperMotorGetState7(void);
float StepperMotorGetCurrentHeight7(void);
static void TestBasicFunction(void);
static void TestDifferentSpeeds(void);
static void TestEmergencyStop(void);
static void TestUpDownCycle(void);
static void TestEntry(void);
static void DelayedMotorTest(void);

// 声明外部可见的函数
extern void motordown(void);
extern void motorstop7(void);

/***************************************************************
* 函数名称: EnableMotor7
* 说    明: 启动电机
* 参    数: enable - true启动电机，false停止电机
* 返 回 值: 无
***************************************************************/
void EnableMotor7(bool enable)
{
    g_motorEnabled = enable;
    if (!enable) {
        StopMotor();
    }
    printf("Motor %s\n", enable ? "Enabled" : "Disabled");
}

// 初始化PWM引脚
static int32_t InitMotorPins(void)
{
    int32_t ret;
    
    // 初始化四个控制引脚
    ret = IoTPwmInit(MOTOR_PIN_A);
    if (ret != 0) {
        printf("PWM0_M1 (GPIO_PB4) initialization failed\n");
        return ret;
    }
    
    ret = IoTPwmInit(MOTOR_PIN_B);
    if (ret != 0) {
        printf("PWM1_M1 (GPIO_PB5) initialization failed\n");
        return ret;
    }
    
    ret = IoTPwmInit(MOTOR_PIN_C);
    if (ret != 0) {
        printf("PWM7_M1 (GPIO_PD0) initialization failed\n");
        return ret;
    }
    
    ret = IoTPwmInit(MOTOR_PIN_D);
    if (ret != 0) {
        printf("PWM2_M1 (GPIO_PB6) initialization failed\n");
        return ret;
    }
    
    printf("All motor pins initialized successfully\n");
    return 0;
}

// 设置电机相序
static void SetMotorPhase(uint8_t phase)
{
    const uint8_t* sequence = STEP_SEQUENCE[phase];
    
    // 打印当前相序状态（简化输出，减少串口负担）
    if (phase == 0) {  // 只在每个完整周期开始时打印
    }
    
    // 设置PWM输出
    IoTPwmStart(MOTOR_PIN_A, sequence[3] ? PWM_DUTY_ON : PWM_DUTY_OFF, PWM_FREQ);
    IoTPwmStart(MOTOR_PIN_B, sequence[2] ? PWM_DUTY_ON : PWM_DUTY_OFF, PWM_FREQ);
    IoTPwmStart(MOTOR_PIN_C, sequence[1] ? PWM_DUTY_ON : PWM_DUTY_OFF, PWM_FREQ);
    IoTPwmStart(MOTOR_PIN_D, sequence[0] ? PWM_DUTY_ON : PWM_DUTY_OFF, PWM_FREQ);
}

// 停止电机
static void StopMotor(void)
{
    // 停止所有PWM输出
    IoTPwmStop(MOTOR_PIN_A);
    IoTPwmStop(MOTOR_PIN_B);
    IoTPwmStop(MOTOR_PIN_C);
    IoTPwmStop(MOTOR_PIN_D);
    g_motorState = MOTOR_STATE_STOPPED;
    printf("Motor stopped at height: %.2f cm\n", g_currentHeight);
}

// 电机控制任务
static void MotorControlTask7(void)
{
    uint32_t delayTime;
    uint8_t phase = 0;
    uint32_t stepCount = 0;
    uint32_t lastPrintTime = 0;
    
    while (1) {
        // 添加启动标志检查
        if (!g_motorEnabled) {
            LOS_Msleep(100);
            continue;
        }

        if (g_motorState == MOTOR_STATE_RUNNING) {
            // 根据速度计算延时时间
            delayTime = STEP_DELAY_BASE * (11 - g_currentSpeed);
            
            // 设置相序
            if (g_currentDirection7 == MOTOR_DIR_UP) {
                phase = (phase + 1) % 4;
            } else {
                phase = (phase + 3) % 4;
            }
            
            // 设置电机相序
            SetMotorPhase(phase);
            g_currentStep++;
            stepCount++;
            
            // 每500ms打印一次状态，避免串口输出过多
            uint32_t currentTime = LOS_TickCountGet();
            if (currentTime - lastPrintTime >= 500) {
                printf("Steps: %d, Height: %.2f cm, Dir: %s, Speed: %d\n",
                       stepCount,
                       g_currentHeight,
                       g_currentDirection7 == MOTOR_DIR_UP ? "UP" : "DOWN",
                       g_currentSpeed);
                lastPrintTime = currentTime;
            }
            
            // 计算当前高度
            float revolutions = (float)g_currentStep / STEPS_PER_REVOLUTION;
            g_currentHeight = revolutions * DISTANCE_PER_REVOLUTION;
            
            // 检查是否达到目标高度
            if (g_currentDirection7 == MOTOR_DIR_UP && g_currentHeight >= g_targetHeight) {
//                StopMotor();
            }
            
            LOS_Msleep(delayTime);
        } else {
            LOS_Msleep(100);
        }
    }
}

// 初始化步进电机
int32_t StepperMotorInit7(void)
{
    printf("Initializing stepper motor...\n");
    
    int32_t ret = InitMotorPins();
    if (ret != 0) {
        printf("Failed to initialize motor pins\n");
        return ret;
    }
    
    // 创建电机控制任务
    uint32_t taskId;
    TSK_INIT_PARAM_S task = {0};
    task.pfnTaskEntry = (TSK_ENTRY_FUNC)MotorControlTask7;
    task.uwStackSize = 2048;
    task.pcName = "MotorControlTask7";
    task.usTaskPrio = 20;
    
    ret = LOS_TaskCreate(&taskId, &task);
    if (ret != LOS_OK) {
        printf("Failed to create motor control task\n");
        return ret;
    }
    
    printf("Stepper motor initialized successfully\n");
    return 0;
}

// 控制步进电机
int32_t StepperMotorControl7(MotorCommand command, float targetHeight, uint8_t speed)
{
    if (speed < 1 || speed > 10) {
        printf("Invalid speed level (1-10): %d\n", speed);
        return -1;
    }
    
    switch (command) {
        case MOTOR_CMD_STOP:
            printf("Stopping motor\n");
            StopMotor();
            break;
            
        case MOTOR_CMD_UP:
            if (targetHeight <= g_currentHeight || targetHeight > 30.0f) {
                printf("Invalid target height: %.2f cm\n", targetHeight);
                return -1;
            }
            printf("Moving up to %.2f cm at speed level %d\n", targetHeight, speed);
            g_targetHeight = targetHeight;
            g_currentDirection7 = MOTOR_DIR_UP;
            g_currentSpeed = speed;
            g_motorState = MOTOR_STATE_RUNNING;
            break;
            
        case MOTOR_CMD_DOWN:
            printf("77Moving down at speed level %d\n", speed);
            g_targetHeight = targetHeight;
            g_currentDirection7 = MOTOR_DIR_DOWN;
            g_currentSpeed = speed;
            g_motorState = MOTOR_STATE_RUNNING;
            break;
            
        default:
            printf("Invalid command\n");
            return -1;
    }
    
    return 0;
}

// 获取电机当前状态
MotorState StepperMotorGetState7(void)
{
    return g_motorState;
}

// 获取当前高度
float StepperMotorGetCurrentHeight7(void)
{
    return g_currentHeight;
}


/***************************************************************
* 函数名称: DelayedMotorTest
* 说    明: 延时启动测试
* 参    数: 无
* 返 回 值: 无
***************************************************************/
static void DelayedMotorTest(void)
{
    printf("\n等待10秒后开始测试...\n");
    LOS_Msleep(10000);  // 延时10秒

    printf("开始30秒运行测试\n");
    EnableMotor7(true);
    StepperMotorControl7(MOTOR_CMD_UP, 10.0f, 8);  // 设置上升10cm，速度等级8

    // 运行30秒
    LOS_Msleep(30000);

    // 停止电机
    EnableMotor7(false);
    printf("30秒测试结束\n");
}



// 修改测试入口函数 - 移除自启动
static void TestEntry(void)
{
    printf("TestEntry: Ready for manual control\n");
}

// 注册到系统启动 - 仅初始化，不自动运行测试
SYS_RUN(TestEntry);

// 修改为外部可见的API函数
// 启动电机
void motordown(void)
{
    printf("\n开始下降电机...\n");
    
    // 初始化电机（如果尚未初始化）
    static bool isInitialized = false;
    if (!isInitialized) {
        if (StepperMotorInit7() != 0) {
            printf("电机初始化失败！\n");
            return;
        }
        isInitialized = true;
        printf("电机初始化成功\n");
    }
    
    // 启动电机
    EnableMotor7(true);
    StepperMotorControl7(MOTOR_CMD_DOWN, 30.0f, 8);  // 设置下降10cm，速度等级8
    printf("电机已启动\n");
}

// 停止电机
void motorstop7(void)
{
    printf("\n停止电机...\n");
    EnableMotor7(false);
    printf("电机已停止\n");
} 
